
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       bitmask.c
  * @author     baiyang
  * @date       2022-11-5
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "bitmask.h"

#include <string.h>

#include <rtthread.h>

#include <internal_error/internal_error.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
static uint16_t mb_bitmask_popcount(uint32_t bits);
static uint16_t mb_bitmask_ffs(uint32_t bits);
#endif
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   bitmask  
  * @param[in]   num_bits  
  * @param[in]   pool  
  * @param[in]   pool_len  
  * @param[out]  
  * @retval      
  * @note        uint32_t pool[(num_bits+31)/32];
  *              #define BITMASK_NUMBITS_TO_NUMWORDS(num_bits) ((num_bits+31)/32)
  */
void mb_bitmask_init(mb_bitmask_t bitmask, uint16_t num_bits, uint32_t *pool)
{
    RT_ASSERT(num_bits > 0);
    RT_ASSERT(bitmask != RT_NULL);

    bitmask->numbits  = num_bits;
    bitmask->numwords = BITMASK_NUMBITS_TO_NUMWORDS(num_bits);
    bitmask->bits     = pool;

    mb_bitmask_clearall(bitmask);
}

#ifdef RT_USING_HEAP
mb_bitmask_t mb_bitmask_create(uint16_t num_bits)
{
    mb_bitmask_t bitmask;
    uint32_t *pool;

    RT_ASSERT(num_bits > 0);

    uint16_t numwords = BITMASK_NUMBITS_TO_NUMWORDS(num_bits);

    bitmask = (struct mb_bitmask *)rt_malloc(sizeof(struct mb_bitmask));
    if (bitmask == RT_NULL)
        goto exit;

    pool = (uint32_t *)rt_malloc(numwords*sizeof(bitmask->bits[0]));
    if (pool == RT_NULL)
    {
        rt_free(bitmask);
        bitmask = RT_NULL;
        goto exit;
    }
    mb_bitmask_init(bitmask, num_bits, pool);

exit:
    return bitmask;
}

void mb_bitmask_destroy(mb_bitmask_t bitmask)
{
    RT_ASSERT(bitmask != RT_NULL);

    rt_free(bitmask->bits);
    rt_free(bitmask);
}
#endif

void mb_bitmask_copy(mb_bitmask_t dst, mb_bitmask_const_t src)
{
    RT_ASSERT(dst->numwords == src->numwords);
    rt_memcpy(dst->bits, src->bits, sizeof(dst->bits[0])*src->numwords);
}

// clear all bits
void mb_bitmask_clearall(mb_bitmask_t bitmask)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    rt_memset(bitmask->bits, 0, bitmask->numwords*sizeof(bitmask->bits[0]));
}

// set given bitnumber
void mb_bitmask_set(mb_bitmask_t bitmask, uint16_t bit)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    // ignore an invalid bit number
    if (bit >= bitmask->numbits) {
        INTERNAL_ERROR(internal_error_bitmask_range);
        return;
    }
    uint16_t word = bit/32;
    uint8_t ofs = bit & 0x1f;
    bitmask->bits[word] |= (1U << ofs);
}

// set all bits
void mb_bitmask_setall(mb_bitmask_t bitmask)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    // set all words to 111... except the last one.
    for (uint16_t i=0; i<bitmask->numwords-1; i++) {
        bitmask->bits[i] = 0xffffffff;
    }
    // set most of the last word to 111.., leaving out-of-range bits to be 0
    uint16_t num_valid_bits = bitmask->numbits % 32;
    bitmask->bits[bitmask->numwords-1] = (1 << num_valid_bits) - 1;
}

// clear given bitnumber
void mb_bitmask_clear(mb_bitmask_t bitmask, uint16_t bit)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    uint16_t word = bit/32;
    uint8_t ofs = bit & 0x1f;
    bitmask->bits[word] &= ~(1U << ofs);
}

// set given bitnumber to on/off
void mb_bitmask_setonoff(mb_bitmask_t bitmask, uint16_t bit, bool onoff)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    if (onoff) {
        mb_bitmask_set(bitmask, bit);
    } else {
        mb_bitmask_clear(bitmask, bit);
    }
}

// return true if given bitnumber is set
bool mb_bitmask_get(mb_bitmask_const_t bitmask, uint16_t bit)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    uint16_t word = bit/32;
    uint8_t ofs = bit & 0x1f;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    if (bit >= bitmask->numbits) {
        INTERNAL_ERROR(internal_error_bitmask_range);
        return false;
    }
#endif
    return (bitmask->bits[word] & (1U << ofs)) != 0;
}

// return true if all bits are clear
bool mb_bitmask_empty(mb_bitmask_const_t bitmask)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    for (uint16_t i=0; i<bitmask->numwords; i++) {
        if (bitmask->bits[i] != 0) {
            return false;
        }
    }
    return true;
}

// return number of bits set
uint16_t mb_bitmask_count(mb_bitmask_const_t bitmask)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    uint16_t sum = 0;
    for (uint16_t i=0; i<bitmask->numwords; i++) {
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        if (sizeof(bitmask->bits[i]) <= sizeof(int)) {
            sum += __builtin_popcount(bitmask->bits[i]);
        } else if (sizeof(bitmask->bits[i]) <= sizeof(long)) {
            sum += __builtin_popcountl(bitmask->bits[i]);
        } else {
            sum += __builtin_popcountll(bitmask->bits[i]);
        }
#else
        sum += mb_bitmask_popcount(bitmask->bits[i]);
#endif
    }
    return sum;
}

// return first bit set, or -1 if none set
int16_t mb_bitmask_first_set(mb_bitmask_const_t bitmask)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    for (uint16_t i=0; i<bitmask->numwords; i++) {
        if (bitmask->bits[i] == 0) {
            continue;
        }
        int fs;
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        if (sizeof(bitmask->bits[i]) <= sizeof(int)) {
            fs = __builtin_ffs(bitmask->bits[i]);
        } else if (sizeof(bitmask->bits[i]) <= sizeof(long)) {
            fs = __builtin_ffsl(bitmask->bits[i]);
        } else {
            fs = __builtin_ffsll(bitmask->bits[i]);
        }
#else
        fs = mb_bitmask_ffs(bitmask->bits[i]);
#endif
        return i*32 + fs - 1;
    }
    return -1;
}

// return number of bits available
uint16_t mb_bitmask_size(mb_bitmask_const_t bitmask)
{
    RT_ASSERT(bitmask != RT_NULL);
    RT_ASSERT(bitmask->bits != RT_NULL);

    return bitmask->numbits;
}

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
// return number of bits available
static uint16_t mb_bitmask_popcount(uint32_t bits)
{
    bits = (bits & 0x55555555) + ((bits >>1)  & 0x55555555);
    bits = (bits & 0x33333333) + ((bits >>2)  & 0x33333333);
    bits = (bits & 0x0f0f0f0f) + ((bits >>4)  & 0x0f0f0f0f);
    bits = (bits & 0x00ff00ff) + ((bits >>8)  & 0x00ff00ff);
    bits = (bits & 0x0000ffff) + ((bits >>16) & 0x0000ffff);

    return bits;
}

// return number of bits available
static uint16_t mb_bitmask_ffs(uint32_t bits)
{
    uint16_t bit = 0;

    if (0 == bits)
        return 0;

    for (bit = 1; !(bits & 1); ++bit) {
        bits >>= 1;
    }

    return bit;
}
#endif
/*------------------------------------test------------------------------------*/


